/***************************************************************************
 *  Filename: maincontroller.h
 *  Class: MainController
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/


#ifndef MAINCONTROLLER_H
#define MAINCONTROLLER_H

#include <QObject>
#include <QMutex>
#include <QTimer>
#include <QTime>

#define Ddp     -6.1024
#define Dpp     9.5061
#define Ppp     -0.4

#define Ddr    -6.1010
#define Dpr    9.5081
#define Ppr    -0.4

#define Dsg     1.0024
#define Psg     -2.9908

#define Py      -2.00055

#define MaxRoll     20.0
#define MaxPitch    20.0
#define MaxYaw      100.0
#define MaxGaz      7.0
class MainController : public QObject
{
    Q_OBJECT
public:
    explicit MainController(QObject *parent = 0);
    
signals:
    void updateCommands(float troll, float tpich, float tyaw, float tgaz);

public slots:
    void updateAnglesData(float troll, float tpitch, float tyaw);
    void updateSpeeds(float tvx, float tvy, float tvz);
    void updateRelativeTarget(float xt, float yt, float zt, float yawt);
    void updateAltitude(float taltitude);
    void controlLoop();
private:
    float targetX; //Relative target X
    float targetY;//Relative target Y
    float targetZ;//Relative target Z
    float targetYaw;//Relative target Yaw

    float roll;
    float pitch;
    float yaw;

    float vx;
    float vy;
    float vz;
    float altitude;
    //float yawRate;

    QMutex mutex;
    QTimer timer;
    QTime speedDerivationTime; //vz
    QTime speedIntegrationTime;//x and y
};

#endif // MAINCONTROLLER_H
